// --------------------------------------------------------------------------------------------------------------------
// <copyright file="SDF.cs" company="Microsoft Corporation">
// The MIT License (MIT)
// 
// Copyright (c) 2014, Microsoft Corporation
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy
//  of this software and associated documentation files (the "Software"), to deal
//  in the Software without restriction, including without limitation the rights
//  to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
//  copies of the Software, and to permit persons to whom the Software is
//  furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in
//  all copies or substantial portions of the Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
//  IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
//  FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
//  AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
//  LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
//  OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
//  THE SOFTWARE.
// </copyright>
// --------------------------------------------------------------------------------------------------------------------
namespace Microsoft.Robotics.Geometry.SDF
{
    using System;
    using System.Runtime.Serialization;
    using System.Threading.Tasks;

    using Microsoft.Robotics.Numerics;
    
    /// <summary>
    /// Class that implements an axis aligned voxel grid that contains a signed distance function
    /// </summary>
    [DataContract]
    public class SDF
    {
        /// <summary>
        /// Initializes a new instance of the SDF class
        /// Constructs a voxel grid of size: floor(size_x / resolution) by floor(size_y / resolution) by floor(size_z / resolution)
        /// </summary>
        /// <param name="size">The size of the voxel grid specified in units (meters, cm, mm, etc)</param>
        /// <param name="resolution">The resolution (in units) of each cell</param>
        public SDF(Vector3 size, double resolution)
        {
            this.Width = (int)(size.X / resolution);
            this.Height = (int)(size.Y / resolution);
            this.Depth = (int)(size.Z / resolution);
            this.Resolution = resolution;
            this.Data = new double[this.Width * this.Height * this.Depth];

            Parallel.For(0, this.Width * this.Height * this.Depth, (i) => this.Data[i] = double.NaN);
        }

        /// <summary>
        /// Initializes a new instance of the SDF class
        /// Constructs a voxel grid of size: x by y by z where each cell has the specified resolution
        /// </summary>
        /// <param name="width">The number of columns in a slice of the 3D volume</param>
        /// <param name="height">The number of rows in a slice of the 3D volume</param>
        /// <param name="depth">The number of width x height 2D grids in the 3D volume</param>
        /// <param name="resolution">The resolution (in units) of each cell</param>
        public SDF(int width, int height, int depth, double resolution)
        {
            this.Width = width;
            this.Height = height;
            this.Depth = depth;
            this.Resolution = resolution;
            this.Data = new double[this.Width * this.Height * this.Depth];

            Parallel.For(0, this.Width * this.Height * this.Depth, (i) => this.Data[i] = double.NaN);
        }

        /// <summary>
        /// Initializes a new instance of the SDF class
        /// Constructs a voxel grid of size: x by y by z where each cell has the specified resolution
        /// </summary>
        /// <param name="width">The number of columns in a slice of the 3D volume</param>
        /// <param name="height">The number of rows in a slice of the 3D volume</param>
        /// <param name="depth">The number of width x height 2D grids in the 3D volume</param>
        /// <param name="resolution">The resolution (in units) of each cell</param>
        /// <param name="data">Initialize with the data</param>
        public SDF(int width, int height, int depth, double resolution, double[] data)
        {
            this.Width = width;
            this.Height = height;
            this.Depth = depth;
            this.Resolution = resolution;
            this.Data = data;
        }

        /// <summary>
        /// Gets the signed distance data
        /// </summary>
        [DataMember(Name = "Data")]
        public double[] Data { get; private set; }

        /// <summary>
        /// Gets the number of columns in a slice of the 3D volume
        /// </summary>
        [DataMember(Name = "Width")]
        public int Width { get; private set; }

        /// <summary>
        /// Gets the number of rows in a slice of the 3D volume
        /// </summary>
        [DataMember(Name = "Height")]
        public int Height { get; private set; }

        /// <summary>
        /// Gets the number of width x height 2D grids in the 3D volume
        /// </summary>
        [DataMember(Name = "Depth")]
        public int Depth { get; private set; }

        /// <summary>
        /// Gets the resolution (in units) of each cell
        /// </summary>
        [DataMember(Name = "Resolution")]
        public double Resolution { get; private set; }

        /// <summary>
        /// Indexes into the voxel grid containing the signed distance data
        /// No tri-linear interpolation, use Interpolate(double x, double y, double z) for that
        /// </summary>
        /// <param name="x">The column of the z slice</param>
        /// <param name="y">The row of the z slice</param>
        /// <param name="z">The slice of the grid</param>
        /// <returns>The signed distance at point (x, y, z) in the voxel grid</returns>
        public double this[int x, int y, int z]
        {
            get
            {
                int index = y * this.Width + x + this.Height * this.Width * z;
                return this.Data[index];
            }

            set 
            {
                int index = y * this.Width + x + this.Height * this.Width * z;
                this.Data[index] = value;
            }
        }

        /// <summary>
        /// Computes the 3D location of corner (x, y, z)
        /// </summary>
        /// <param name="x">The column of the z slice</param>
        /// <param name="y">The row of the z slice</param>
        /// <param name="z">The slice of the grid</param>
        /// <returns>The 3D location of corner (x, y, z)</returns>
        public Vector3 Get3DPoint(int x, int y, int z)
        {
            return new Vector3(
                (double)(x - this.Width / 2) * this.Resolution,
                (double)(y - this.Height / 2) * this.Resolution,
                (double)(z - this.Depth / 2) * this.Resolution);
        }

        /// <summary>
        /// Computes the location in the grid given a 3D point
        /// </summary>
        /// <param name="point">The 3D point</param>
        /// <returns>The location in the grid</returns>
        public Vector3 GetGridPoint(Vector3 point)
        {
            return new Vector3(
                point.X / this.Resolution + this.Width / 2,
                point.Y / this.Resolution + this.Height / 2,
                point.Z / this.Resolution + this.Depth / 2);
        }

        /// <summary>
        /// Determines if a 3D point is contained inside the bounding box (including the faces)
        /// </summary>
        /// <param name="point">The 3D point</param>
        /// <returns>A boolean, false means the point is outside the volume</returns>
        public bool Contains(Vector3 point)
        {
            Vector3 gridPoint = this.GetGridPoint(point);
            gridPoint.X = Math.Round(gridPoint.X, 3);
            gridPoint.Y = Math.Round(gridPoint.Y, 3);
            gridPoint.Z = Math.Round(gridPoint.Z, 3);
            if (gridPoint.X >= 0 && gridPoint.X <= this.Width && gridPoint.Y >= 0 && gridPoint.Y <= this.Height && gridPoint.Z >= 0 && gridPoint.Z <= this.Depth)
            {
                return true;
            }

            return false;
        }

        /// <summary>
        /// Interpolates a value in the voxel grid
        /// </summary>
        /// <param name="point">The grid Point</param>
        /// <returns>The interpolated value</returns>
        public double Interpolate(Vector3 point)
        {
            int x0, x1;
            int y0, y1;
            int z0, z1;

            x0 = (int)point.X;
            x1 = (int)(point.X + 1);

            y0 = (int)point.Y;
            y1 = (int)(point.Y + 1);

            z0 = (int)point.Z;
            z1 = (int)(point.Z + 1);

            if (x0 < 0 || x1 >= this.Width || y0 < 0 || y1 >= this.Height || z0 < 0 || z1 >= this.Depth)
            {
                return double.NaN;
            }

            double xd = (point.X - (double)x0) / ((double)x1 - (double)x0);
            double yd = (point.Y - (double)y0) / ((double)y1 - (double)y0);
            double zd = (point.Z - (double)z0) / ((double)z1 - (double)z0);

            double c00 = this[x0, y0, z0] * (1.0 - xd) + this[x1, y0, z0] * xd;
            double c10 = this[x0, y1, z0] * (1.0 - xd) + this[x1, y1, z0] * xd;
            double c01 = this[x0, y0, z1] * (1.0 - xd) + this[x1, y0, z1] * xd;
            double c11 = this[x0, y1, z1] * (1.0 - xd) + this[x1, y1, z1] * xd;

            double c0 = c00 * (1 - yd) + c10 * yd;
            double c1 = c01 * (1 - yd) + c11 * yd;

            return c0 * (1 - zd) + c1 * zd;
        }

        /// <summary>
        /// Evaluates the grid at the 3D point
        /// </summary>
        /// <param name="point">The 3D point to evaluate</param>
        /// <returns>The evaluated 3D point</returns>
        public double Eval(Vector3 point)
        {
            return this.Interpolate(this.GetGridPoint(point));
        }

        /// <summary>
        /// Evaluates the gradient at a 3D point
        /// </summary>
        /// <param name="point">The 3D point to evaluate</param>
        /// <returns>The gradient</returns>
        public Vector3 Gradient(Vector3 point)
        {
            Vector3 gridPoint = this.GetGridPoint(point);

            double epsilon = 0.01;
            double gradX = (this.Interpolate(gridPoint + new Vector3(epsilon, 0, 0)) - this.Interpolate(gridPoint - new Vector3(epsilon, 0, 0))) / (2 * epsilon);
            double gradY = (this.Interpolate(gridPoint + new Vector3(0, epsilon, 0)) - this.Interpolate(gridPoint - new Vector3(0, epsilon, 0))) / (2 * epsilon);
            double gradZ = (this.Interpolate(gridPoint + new Vector3(0, 0, epsilon)) - this.Interpolate(gridPoint - new Vector3(0, 0, epsilon))) / (2 * epsilon);

            if (double.IsNaN(gradX) || double.IsNaN(gradY) || double.IsNaN(gradZ))
            {
                return new Vector3(double.NaN, double.NaN, double.NaN);
            }

            return new Vector3(gradX, gradY, gradZ);
        }

        /// <summary>
        /// Computes the volume of the grid
        /// </summary>
        /// <returns>Gets the volume</returns>
        public double Volume()
        {
            return (this.Width * this.Height * this.Depth) * (this.Resolution * this.Resolution * this.Resolution);
        }
    }
}
